#include <stdio.h>
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include <opencv2/video/video.hpp>
#include <opencv2/opencv.hpp>
#include "camera_class.h"
#include <iostream>
#include <cmath>
#include <cstdlib>

using namespace cv;
using namespace std;

float P_yaw;
float P_pitch;

bool color_mode = true;

double dis(Point p1, Point p2){  //求两点距离
    double dis;
    dis = sqrt(pow(abs(p1.x - p2.x), 2) + pow(abs(p1.y - p2.y), 2));
    return dis;
}

int main()
{
    Mat image;    //声明目标图像变量
    int key;
    camera cam;
    cam.start_cam();
    while (1)
    {
        cam.get_pic(&image);
        flip(image, image, 2);  //图像翻转
        if(image.empty())
        break;

        resize(image, image, Size(640, 480));

        imshow("Display Image", image);
    
        Mat img;
        image.copyTo(img);      //深拷贝（缺陷 较为消耗算力）

        int color;              //颜色模式判断
        if(color_mode == true)
            color = 0;          //蓝色模式
        else
            color = 2;          //红色模式      !!!阈值与蓝色有差别，possibly需要重新调参

        for(int i = 0; i < image.cols - 1; i++){             //消除白色影响（非线性滤波）
            for(int j = 0; j < image.rows - 1; j++)
            {
                int b = image.at<Vec3b>(j,i)[0];
                int g = image.at<Vec3b>(j,i)[1];
                int r = image.at<Vec3b>(j,i)[2];
                if(b > 120 && g > 120 && r > 120)
                {
                    image.at<Vec3b>(j,i)[0] = 0;
                    image.at<Vec3b>(j,i)[1] = 0;
                    image.at<Vec3b>(j,i)[2] = 0;
                }
            }
        }

        Mat result1;
        vector<Mat> channels;
        split(image, channels);     //通道分离
        Mat B_R = channels.at(color);  //提取颜色通道

        Mat th_b;               //二值化处理
        threshold(B_R, th_b, 210, 255, THRESH_BINARY);     //阈值给定考虑：180，210阈值在后续两种滤波之下会消除最小灯条轮廓

        Mat resultG;
        medianBlur(th_b, th_b, 11);                 //中值滤波
        GaussianBlur(th_b, resultG, Size(3,5), 3);  //高斯低通滤波
        Canny(resultG, resultG, 100, 200, 3);       //Canny算法

        //最小外接矩形 + 矩的计算
        vector<vector<Point>> contours;
        findContours(resultG, contours, 0, 2);  //轮廓点检测
        cout << "轮廓点数：" << contours.size() <<endl;

        for(int i = contours.size() - 1; i >= 0; i--){      //筛选单个符合几何条件矩形点位(对单个矩形限制：长宽比、角度)
            RotatedRect rrect = minAreaRect(contours[i]);
            Point2f points[4];
            rrect.points(points);
            Point2f cpt = rrect.center;

            if(((dis(points[0], points[1]) / dis(points[1], points[2])) > 2.0 || (dis(points[1], points[2]) / dis(points[1], points[0])) > 2.0 )    //长宽比条件限制
            && (rrect.angle > -25 || rrect.angle < -65)     //角度限制
            ){
                i -= 1;
                continue;
            }
                
            if(i >= 0){                     //pop出不符合条件点位
                auto it = contours.begin() + i;
                contours.erase(it);
            }
        }

        vector<RotatedRect> Lrect;     //定义灯条中心点vector

        for(int i = contours.size() - 1; i >= 0; i--){      //筛选灯条相关矩形点位(对多个矩形进行相关比较)
            RotatedRect rrect = minAreaRect(contours[i]);
            Point2f points[4];
            rrect.points(points);
            Point2f cpt = rrect.center;

            bool judge = false;  //条件判断标志位设立

            for(int j = contours.size() -1; j >= 0; j--){    //再次遍历contours，对coutours[i]进行相关性比较
                RotatedRect nrect = minAreaRect(contours[j]);
                Point2f pointn[4];
                nrect.points(pointn);
                Point2f cpn = nrect.center;

                int blue = img.at<Vec3b>(cpn)[0];
                int green = img.at<Vec3b>(cpn)[1];
                int red = img.at<Vec3b>(cpn)[2];

                if(i == j)      //排除自身的相关性
                    continue;
                
                if(1){    //条件判断
                    judge = true;      //条件满足标志位赋值
                    Lrect.push_back(rrect);    //中心点位存入
                    circle(img, cpn, 2, Scalar(0,0,255), 2, 8, 0);  //中心点标记
                    break;
                }
            }

            if(judge == true)     //满足条件判断
                continue;

            if(i >= 0){         //移除不满足条件的矩形
                auto it = contours.begin() + i;
                contours.erase(it);
            }
        }

        cout << "轮廓点数：" << contours.size() <<endl;
   
        vector<vector<Point2f>> B_points;   //定义装甲板点位矩形变量
        
        for(int i = 0; i < Lrect.size(); i++){      //装甲板矩形获取

            for(int j = 0; j < Lrect.size(); j++){
                if(i == j)
                    continue;

                if(abs(Lrect[i].angle - Lrect[j].angle) < 20 && abs(Lrect[i].center.y - Lrect[j].center.y) < 50){  //限制条件判断
                    Point2f cen((Lrect[i].center.x + Lrect[j].center.x)/2, (Lrect[i].center.y + Lrect[j].center.y)/2);  //取理论上亮灯条中心点

                    //points get
                    vector<Point2f> tmp_p;
                    tmp_p.push_back(cen);
                    tmp_p.push_back(Point(Lrect[i].center.x, Lrect[i].center.y + Lrect[i].size.width / 2));    //LIght 1
                    tmp_p.push_back(Point(Lrect[i].center.x, Lrect[i].center.y - Lrect[i].size.width / 2));    //Light 1 finished
                    tmp_p.push_back(Point(Lrect[j].center.x, Lrect[j].center.y - Lrect[j].size.width / 2));    //Light 2
                    tmp_p.push_back(Point(Lrect[j].center.x, Lrect[j].center.y + Lrect[j].size.width / 2));    //Light 2 finished

                    B_points.push_back(tmp_p);

                    P_yaw = (320 - cen.x) * 0.01;
                    P_pitch = (240 - cen.y) * 0.01;
                }
            }
        }

        for(int i = 0; i < B_points.size(); i++){   //遍历Boa，装甲板矩形勾画
            Point2f points[4];
            points[0] = B_points[i][1];
            points[1] = B_points[i][2];
            points[2] = B_points[i][3];
            points[3] = B_points[i][4];
            Point2f cpt = B_points[i][0];

            for(int j = 0; j < 4; j++)          //框出最小矩形
            {
                if(j == 3)      //首尾相接
                {
                    line(img, points[j], points[0], Scalar(255,0,255), 2, 8, 0);
                    break;
                }
                line(img, points[j], points[j+1], Scalar(255,0,255), 2, 8, 0);    //接出三边
            }
            circle(img, cpt, 2, Scalar(255,0,255), 2, 8, 0);  //中心点标记   
        }

        imshow("result", img);
            
            key=waitKey(1);
            if(key==27)
            {
                cam.close_cam();
                break;
            }
    }
    
    waitKey(3000);

    return 0;
}